
function DRO_test
% 
% 绘制DRO轨道簇
% 
% 作者：张晨
% 单位：中科院空间应用工程与技术中心
% 时间：2021年3约30日
% chenzhang@csu.ac.cn
% -----------------------------------------------------------

clc; clear; close all;

% 地月引力常数
mu = 0.01211; 

% DRO轨道簇
droX0Mtx = [0.005303995654298                         0      9.95154501480999e-07                         0          10.5880299768115                         0          6.30749706595705
    0.0866399960493618                         0      3.10612258763273e-08                         0            4.263680750635                         0          6.29645952152456
    0.167975996444426                         0      5.44635929356846e-08                         0          2.98251106413455                         0          6.27911759318159
    0.249311996839489                         0      9.23461865697633e-08                         0          2.30720873365635                         0           6.2507901690156
    0.330647997234553                         0     -3.48502863312205e-10                         0          1.85176435543791                         0          6.20421748534132
    0.411983997629617                         0      5.53616791096806e-09                         0          1.50598724718544                         0          6.12605000178816
    0.493319998024681                         0      1.42530812000754e-09                         0          1.22535565415162                         0          5.99022260330428
    0.574655998419745                         0      3.88951156958366e-10                         0         0.989014218061593                         0          5.74335048016289
    0.655991998814809                         0      7.14661575768379e-11                         0          0.78788243470929                         0          5.27508350639483
    0.737327999209872                         0     -6.42039213331429e-11                         0         0.622222451512701                         0          4.38973919426031
    0.818663999604936                         0     -2.70700168824891e-10                         0         0.504644281005582                         0          2.95052212484007
    0.9                         0     -2.75535754030073e-09                         0           0.4763914772214                         0          1.27956964348298];


% 旋转系画图
figure(1); hold on; grid on; axis equal;

% 画EM系统
crtbpMarkEM;

for iLoop = 1 : size(droX0Mtx , 1)
    
    % 提取初值和周期
    droXxi = droX0Mtx(iLoop , 1:6);
    droPeriod = droX0Mtx(iLoop , 7);
    
    % 画DRO
    crtbpFlow3D(droXxi , droPeriod , mu);
    
end

end

function [tt , xx] = crtbpFlow3D(xx0 , tf , mu)
%
% crtbp三维数值积分
%
% 例子：
% crtbpMarkEM
% aux = crtbpAuxEM;
% xx0 = [0.75 , 0 , 0 , 0.15 , 0 , 0];
% crtbpFlow3D(xx0 , 10 , aux.mu)
%
% 作者：张晨
% 单位：中科院空间应用工程与技术中心
% 时间：2021年3约30日
% chenzhang@csu.ac.cn
% -----------------------------------------------------------

options = odeset('Reltol', 1e-12, 'AbsTol',1e-12);

[tt , xx]=ode113(@fx , [0 , tf] , xx0 , options , mu);
plot3(xx(: , 1) , xx(: , 2) , xx(: , 3) , 'b');

% 动力学方程
    function dxxdt = fx (t, xx, mu)
        r1cube = ((xx(1) + mu)^2 + xx(2)^2 + xx(3)^2)^(1.5);
        r2cube = ((xx(1) - 1 + mu)^2 + xx(2)^2 + xx(3)^2)^(1.5);
        dxxdt = [xx(4);
            xx(5);
            xx(6);
            xx(1) + 2*xx(5) - (1 - mu) * (xx(1) + mu) / r1cube - mu*(xx(1) - 1 + mu) / r2cube;
            xx(2) - 2*xx(4) - (1 - mu) * xx(2) / r1cube - mu * xx(2) / r2cube;
            -(1 - mu) * xx(3) / r1cube - mu * xx(3) / r2cube];
    end

end

function crtbpMarkEM
%
% 画地球和月球
% 
% 作者：张晨
% 单位：中科院空间应用工程与技术中心
% 时间：2021年3约30日
% chenzhang@csu.ac.cn
% -----------------------------------------------------------

% leoFlag = 1; % 画leo
leoFlag = 0; % 画leo

% lloFlag = 1; % 画llo
lloFlag = 0; % 画llo

liFlag = 1; % 画平动点
% liFlag = 0; % 画平动点

% -------------------------------------------------------------
mu = 0.0121506683;
LU = 384405; % [km]

rSun = 6.955e5; % 太阳半径
rEarth = 6378; % 地球半径
rMoon = 1737; % 月球半径

altiLeo = 200; % 近地停泊轨道高度
altiLLO = 200; % 近月目标轨道高度

% ------------------------------------------------------------
% 画地球和月球
[xs , ys , zs] = sphere(30) ;
surf(-mu+xs*rEarth / LU,  ys*rEarth / LU, zs*rEarth / LU, 'FaceColor', 'w') ; hold on;
surf(1-mu+xs*rMoon / LU, ys*rMoon / LU, zs*rMoon / LU, 'FaceColor', 'w') ; hold on;

text( -mu,  0, 0, 'Earth') ; hold on;
text(1 - mu,  0, 0, 'Moon') ; hold on;

% 画LEO
if leoFlag == 1
    ri = (rEarth + altiLeo) / LU; % 近地停泊轨道地心距
    angle = 0 : 0.05 : 2*pi + 0.05 ;
    plot(-mu + ri*cos(angle), ri*sin(angle), 'k-.') ; hold on;
end

% 画LLO
if lloFlag == 1
    rf = (rMoon + altiLLO) / LU; % 近月目标轨道月心距
    plot(1-mu + rf*cos(angle), rf*sin(angle), 'k-.') ; hold on;
end

% 画平动点
if liFlag == 1
    [Li_pos , ~] = crtbpLi(mu);
    plot(Li_pos(1,1) , Li_pos(1,2) , 'r.' , 'linewidth' , 1);
    plot(Li_pos(2,1) , Li_pos(2,2) , 'r.' , 'linewidth' , 1);
    plot(Li_pos(3,1) , Li_pos(3,2) , 'r.' , 'linewidth' , 1);
    plot(Li_pos(4,1) , Li_pos(4,2) , 'r.' , 'linewidth' , 1);
    plot(Li_pos(5,1) , Li_pos(5,2) , 'r.' , 'linewidth' , 1);
    
    text(Li_pos(1,1) , Li_pos(1,2) , 'L1' , 'linewidth' , 1);
    text(Li_pos(2,1) , Li_pos(2,2) , 'L2' , 'linewidth' , 1);
    text(Li_pos(3,1) , Li_pos(3,2) , 'L3' , 'linewidth' , 1);
    text(Li_pos(4,1) , Li_pos(4,2) , 'L4' , 'linewidth' , 1);
    text(Li_pos(5,1) , Li_pos(5,2) , 'L5' , 'linewidth' , 1);
end

axis equal;
grid on;
xlabel('x (LU)');
ylabel('y (LU)');
zlabel('z (LU)');
view(0 , 90);

end

function [Li_pos , Li_c] = crtbpLi(mu)
% 计算crtbp的5个拉格朗日点和能量
%
% 例子：
% % [1]
% aux = crtbpAuxEM;
% [Li_pos , Li_c] = crtbpLi(aux.mu)
%
% % [2]
% aux = crtbpAuxSE;
% [Li_pos , Li_c] = crtbpLi(aux.mu)
%
% 作者：张晨
% 单位：中科院空间应用工程与技术中心
% 时间：2021年3约30日
% chenzhang@csu.ac.cn
% -----------------------------------------------------------
% 设置初始猜测值
x012 = (mu/3)^(1/3) ;
x03  = 1-(7/12)*mu  ;
% 计算5个平动点坐标
options = optimset('TolFun', 2.5e-14, 'TolX', 2.5e-14) ;
L1  = 1 - mu - fzero(@(x) x^5-(3-mu)*x^4+(3-2*mu)*x^3-mu*x^2+2*mu*x-mu, ...
    x012, options) ;
L2  = 1 - mu + fzero(@(x) x^5+(3-mu)*x^4+(3-2*mu)*x^3-mu*x^2-2*mu*x-mu, ...
    x012, options) ;
L3  =   - mu - fzero(@(x) x^5+(2+mu)*x^4+(1+2*mu)*x^3-(1-mu)*x^2-2*(1-mu)*x...
    -(1-mu), x03,  options) ;
L4x = 0.5 - mu ;
L4y = sqrt(3)/2 ;
% 构造5个平动点位置
Li_pos = [L1, 0;
    L2, 0;
    L3, 0;
    L4x, L4y;
    L4x, -L4y];
% 如果输出大于1则计算5个平动点能量
if nargout > 1
    xx = [Li_pos , zeros(5 , 2)];
    Li_c = crtbpJacobi2D(xx , mu);
end
end

function CC = crtbpJacobi2D (xx, mu)
% 计算雅可比能量C
%
% 输入：
% xx       [nx4]
% 输出：
% C        [nx1]
%
% 作者：张晨
% 单位：中科院空间应用工程与技术中心
% 时间：2021年3约30日
% chenzhang@csu.ac.cn
% -----------------------------------------------------------
if size(xx , 2) ~= 4
    disp('xx是n * 4')
end
r1 = sqrt((xx(: , 1)+mu).^2   + xx(: , 2).^2) ;
r2 = sqrt((xx(: , 1)+mu-1).^2 + xx(: , 2).^2) ;
CC = -(xx(: , 3).^2 + xx(: , 4).^2) ...
    + (xx(: , 1).^2 + xx(: , 2).^2) ...
    + 2 * (1 - mu) ./ r1 + 2 * mu ./ r2;
end
